#include <VirtualRobot/RuntimeEnvironment.h>

#include <boost/extension/shared_library.hpp>
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/rolling_mean.hpp>
#include <string>
#include <map>
#include <iostream>
#include <fstream>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <MMM/Motion/Legacy/LegacyMotionReaderXML.h>
#include "XMLMotionCompleterConfiguration.h"

using std::cout;
using std::endl;
using namespace boost::accumulators;


int main(int argc, char *argv[])
{
    cout << " --- XMLMotionCompleter --- " << endl;
    XMLMotionCompleterConfiguration c;
    if (!c.processCommandLine(argc,argv))
    {
        cout << "Error while processing command line, aborting..." << endl;
        return -1;
    }

    MMM::LegacyMotionReaderXMLPtr r(new MMM::LegacyMotionReaderXML());
    std::vector < std::string > motionNames = r->getMotionNames(c.motionFile);

    if (motionNames.size() == 0)
    {
        MMM_ERROR << "no motions in file " << endl;
        return -1;
    }

    int frameCount = -1;
    for(size_t i = 0; i < motionNames.size(); i++)
    {
        MMM::LegacyMotionPtr motion = r->loadMotion(c.motionFile,motionNames[i]);
        if(!motion)
            continue;
        cout << "Loading motion: " << motion->getName() << endl;
        if(frameCount != -1 && frameCount != (int) motion->getNumFrames())
        {
            MMM_ERROR << "Error whole loading motions: the frame count between two motions do not match. All motions need to have the same framecount." << endl;
            return -1;
        }
        else
            frameCount = motion->getNumFrames();

        if (motion)
        {
            int windowSize = 10;  // TODO hardcoded

            // Smooth poses quick and dirty
            // For angles see http://stackoverflow.com/questions/491738/how-do-you-calculate-the-average-of-a-set-of-angles
            // Boost cannot handle Eigen::Vector3f out of the box, this is why I am using floats here
            // (see: http://stackoverflow.com/questions/7908982/using-boost-accumulators-with-eigenvector-types)

            std::vector<MMM::MotionFramePtr> frames = motion->getMotionFrames();
            std::vector<accumulator_set<float, stats<tag::rolling_mean> > > accumulators_pos;
            std::vector<accumulator_set<float, stats<tag::rolling_sum> > > accumulators_rot;

            for (int i = 0; i < 3; ++i)
            {
                accumulators_pos.push_back(accumulator_set<float, stats<tag::rolling_mean> >(tag::rolling_window::window_size = windowSize));
            }
            for (int i = 0; i < 6; ++i)
            {
                accumulators_rot.push_back(accumulator_set<float, stats<tag::rolling_sum> >(tag::rolling_window::window_size = windowSize));
            }

            for (std::vector<MMM::MotionFramePtr>::const_iterator i = frames.begin(); i != frames.end(); ++i)
            {
                MMM::MotionFramePtr frame = *i;

                Eigen::Vector3f pos = frame->getRootPos(), rot = frame->getRootRot();

                accumulators_pos[0](pos[0]); accumulators_pos[1](pos[1]); accumulators_pos[2](pos[2]);

                accumulators_rot[0](sin(rot[0])); accumulators_rot[1](cos(rot[0]));
                accumulators_rot[2](sin(rot[1])); accumulators_rot[3](cos(rot[1]));
                accumulators_rot[4](sin(rot[2])); accumulators_rot[5](cos(rot[2]));

                Eigen::Vector3f smoothed_pos;
                smoothed_pos[0] = rolling_mean(accumulators_pos[0]);
                smoothed_pos[1] = rolling_mean(accumulators_pos[1]);
                smoothed_pos[2] = rolling_mean(accumulators_pos[2]);

                Eigen::Vector3f smoothed_rot;
                smoothed_rot[0] = atan2(rolling_sum(accumulators_rot[0]), rolling_sum(accumulators_rot[1]));
                smoothed_rot[1] = atan2(rolling_sum(accumulators_rot[2]), rolling_sum(accumulators_rot[3]));
                smoothed_rot[2] = atan2(rolling_sum(accumulators_rot[4]), rolling_sum(accumulators_rot[5]));

                /* std::cout << "pos:     orig = " << pos(0) << " " << pos(1) << " " << pos(2) << std::endl;
                std::cout << "     smoothed = " << smoothed_pos(0) << " " << smoothed_pos(1) << " " << smoothed_pos(2) << std::endl;
                std::cout << "rot:     orig = " << rot(0) << " " << rot(1) << " " << rot(2) << std::endl;
                std::cout << "     smoothed = " << smoothed_rot(0) << " " << smoothed_rot(1) << " " << smoothed_rot(2) << std::endl; */

                frame->setRootPos(smoothed_pos);
                frame->setRootRot(smoothed_rot);
            }


            // TODO: insert check which of the motion parameters have to be calculated (positions, velocities, accelerations

            motion->smoothJointValues(MMM::LegacyMotion::eValues, windowSize);
            motion->calculateVelocities();
            motion->smoothJointValues(MMM::LegacyMotion::eVelocities, windowSize);
            motion->calculateAccelerations();
            motion->smoothJointValues(MMM::LegacyMotion::eAccelerations, windowSize);

            std::string motionXML = motion->toXML();
            std::string filename = c.motionFile;

            boost::filesystem::path tmppath = boost::filesystem::canonical(boost::filesystem::path(filename));
            boost::filesystem::path path = tmppath.remove_filename() / boost::filesystem::path(tmppath.stem().generic_string() +"_completed.xml");

            std::string filename_completed = path.generic_string();

            if (!MMM::XML::saveXML(filename_completed, motionXML))
            {
                MMM_ERROR << " Could not write to file " << filename_completed << endl;
            }
            /**std::vector<std::string> jointNames = motion->getJointNames();
            //if (jointNames.size()>0)
            {
                std::string rnsName("MMMViewerRNS");
                if (robot->hasRobotNodeSet(rnsName))
                    robot->deregisterRobotNodeSet(robot->getRobotNodeSet(rnsName));
                robotNodeSets[motion->getName()] = RobotNodeSet::createRobotNodeSet(robot, rnsName, jointNames, "", "", true);
            }
            UI.horizontalSliderFrame->setRange(0, motion->getNumFrames());
            setupTable(); **/
        }
        //motions.push_back(motion);
    }
    cout << "Completing motion data... done..." << endl;
    return 0;
}
